#include <opencv2/opencv.hpp>
#include "opencv2/imgproc/imgproc_c.h"

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

#include <iostream>

using namespace cv;
using namespace std;

struct userdata{
    Mat im;
    vector<Point2f> points;
};


void mouseHandler(int event, int x, int y, int flags, void* data_ptr)
{
    if  ( event == EVENT_LBUTTONDOWN )
    {
        userdata *data = ((userdata *) data_ptr);
        circle(data->im, Point(x,y),3,Scalar(0,255,255), 5, CV_AA);
        imshow("Image", data->im);
        if (data->points.size() < 4)
        {
            data->points.push_back(Point2f(x,y));
        }
    }
    
}

int main( int argc, char** argv)
{

    // Read in the image.
    Mat im_src = imread("ad.jpg");// 输入广告图片
    Mat im_ad = imread("ad.jpg");

    Size size = im_src.size();
   
    // Create a vector of points.
    // y x --- v u 的存储
    vector<Point2f> pts_src;
    pts_src.push_back(Point2f(0,0));// 左上
    pts_src.push_back(Point2f(size.width - 1, 0));// 右上
    pts_src.push_back(Point2f(size.width - 1, size.height -1));// 右下 
    pts_src.push_back(Point2f(0, size.height - 1 ));// 左下
    
    // Set data for mouse handler
    Mat im_temp = im_ad.clone();
    userdata data;
    data.im = im_temp;

    //show the image
    imshow("Image", im_temp);
    
    cout << "Click on four corners of a billboard and then press ENTER" << endl;
    cout << "请按照左上 右上 右下 左下的顺序点击" << endl;
    setMouseCallback("Image", mouseHandler, &data);
    //注意鼠标旋转顺序和原图定义角点顺序要一致
    while(1){    
        waitKey(0);
        if(data.points.size() != 4){
            data.points.clear();
            cout << "please re click" << endl;
        }else{
            break;
        }
    }
    

    std::cout << "开始计算H矩阵" << std::endl;
    
    // 使用dlt计算H矩阵 reference slam book p172
    Eigen::Matrix3d H_Mat;
    Eigen::Matrix<double, 8, 8> A_Mat;
    A_Mat.setZero();
    Eigen::Matrix<double, 8, 1> B_Mat;
    B_Mat.setZero();
    for(int i = 0; i < 4; ++i){
        double u1 = pts_src[i].x;
        double v1 = pts_src[i].y;
        double u2 = data.points[i].x;
        double v2 = data.points[i].y;
        A_Mat.block<1, 8>(i*2, 0) << u1, v1, 1, 0, 0, 0, -u1 * u2, -v1 * u2;
        A_Mat.block<1, 8>(i*2+1, 0) << 0, 0, 0, u1, v1, 1, -u1 * v2, -v1 * v2;

        B_Mat(i*2, 0) = u2;
        B_Mat(i*2+1, 0) = v2; 
    }
    // (A^T * A)^-1 A^T * b  最小二乘

    // Eigen::Matrix<double, 8, 1> x = (A_Mat.transpose() * A_Mat).inverse() * A_Mat.transpose() * B_Mat; 
    // 采用ldlt分解求 Ax = b   A^T A x = A^T b 
    Eigen::Matrix<double, 8, 8> ATA = A_Mat.transpose() * A_Mat;
    Eigen::Matrix<double, 8, 1> ATB = A_Mat.transpose() * B_Mat;
    Eigen::Matrix<double, 8, 1> x = ATA.ldlt().solve(ATB);
    
    H_Mat << x(0,0), x(1,0), x(2,0), x(3,0), x(4,0), x(5,0), x(6,0), x(7,0), 1; 

    // checkH
    for(int i = 0; i < 4; i++){
        Eigen::Vector3d ptsPoint(pts_src[i].x, pts_src[i].y, 1);
        Eigen::Vector3d dataPoint = H_Mat * ptsPoint;
        Eigen::Vector3d targetPoint(data.points[i].x, data.points[i].y, 1);
        dataPoint = dataPoint/dataPoint[2];
        // cout << "target: " << targetPoint.transpose() << " || used: " << ptsPoint.transpose() << 
        //         " ||now:" << dataPoint.transpose() << "||" << endl;
        if((dataPoint - targetPoint).norm() > 15){
            cout << "!!! error H_mat" << endl;
            return 1;
        }
    }
    // 用H
    Size sizeTarget = im_ad.size();

    for(int x = 0; x < size.width; x++){
        for(int y = 0; y < size.height; y++){

            Eigen::Vector3d ptsPoint(x, y, 1);
            Eigen::Vector3d dataPoint = H_Mat * ptsPoint;
            dataPoint = dataPoint/dataPoint[2];
            // in picture
            if(dataPoint[0] < 0) continue;
            if(dataPoint[0] >= sizeTarget.width) continue;
            if(dataPoint[1] < 0) continue;
            if(dataPoint[1] >= sizeTarget.height) continue;
            
            im_ad.at<Vec3b>(dataPoint[1], dataPoint[0])[0] = im_src.at<Vec3b>(y, x)[0];
            im_ad.at<Vec3b>(dataPoint[1], dataPoint[0])[1] = im_src.at<Vec3b>(y, x)[1];
            im_ad.at<Vec3b>(dataPoint[1], dataPoint[0])[2] = im_src.at<Vec3b>(y, x)[2];
        }
    }
    // Display image.
    imshow("Image", im_ad);
    waitKey(0);

    return 0;
}
